/*
 * CostMarker.cpp
 *
 *  Created on: Oct 21, 2010
 *      Author: sanjeev
 */

#include "CostMarker.h"
#include "constants.h"

CostMarker::CostMarker() {
	point.header.frame_id = "/map";

	point.ns = "costs";
	point.action = visualization_msgs::Marker::ADD;
	point.pose.orientation.w = 1.0;

	point.id = 0;

	point.type = visualization_msgs::Marker::POINTS;

	// point markers use x and y scale for width/height respectively
	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.scale.z = 0.1f;

	// point are yellow
	point.color.g = 1.0f;
	point.color.r = 1.0f;
	point.color.a = 1.0f;
	point.lifetime = ros::Duration(0);
}

CostMarker::~CostMarker() {
	// TODO Auto-generated destructor stub
}

void CostMarker::PlotParticles(RobotSet &S,ros::Publisher &pub) {
	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.ns = "target";

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;

	point.points.clear();
	double mapX, mapY, mapTheta;

	for (int i = 0, cnt = 0; i < S.size(); i++) {
		for (int j = 0; j < S[i].size(); j++) {
			S[i][j].getPoseInMap(mapX, mapY, mapTheta);
			c.r = 1.0;
			c.b = 0.0;
			c.g = 0.0;
			c.a = 1.0;
			p.x = mapX;
			p.y = mapY;
			//ROS_INFO("Plot point %lf %lf",p.x,p.y);
			p.z = 0;
			point.colors.push_back(c);
			point.points.push_back(p);
		}
	}
	pub.publish(point);

}

void CostMarker::Plot(MATRIX &inp, ros::Publisher &pub, double grain) {
	point.points.clear();
	int height = inp.size();
	int width = inp[0].size();

	double min = INF, max = -INF, temp;

	for (int i = 0; i < height; i++) {
		for (int j = 0; j < width; j++) {
			min = inp[i][j] > 0.0 ? std::min(min, inp[i][j]) : min;
			max = inp[i][j] < INF / 10 ? std::max(max, inp[i][j]) : max;
		}
	}

	//ROS_INFO("Plotting with max as %lf and min as %lf",max,min);

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	for (int i = 0; i < height; ++i) {
		for (int j = 0; j < width; ++j) {
			temp = inp[i][j];
			if (temp == 0 || temp == INF) {
				continue;
			} else {
				c.r = (temp - min + 1) / (max - min + 1);
				c.b = (max - temp + 1) / (max - min + 1);
				//c.g = 0.0;
			}
			c.a = 1.0;
			p.x = j * grain;
			p.y = i * grain;
			p.z = 0;
			point.colors.push_back(c);
			point.points.push_back(p);
		}
	}
	pub.publish(point);
}

void CostMarker::PlotPoseProbability(PoseArray &pa, WeightsForPoses &w, ros::Publisher &pub,
		double grain) {
	point.points.clear();
	int len = pa.size();

	double min = INF, max = -INF, temp;

	for (int i = 0; i < len; i++) {
		min = w[i] > 0.0 ? std::min(min, w[i]) : min;
		max = w[i] < INF / 10 ? std::max(max, w[i]) : max;
	}

	//ROS_INFO("Plotting with max as %lf and min as %lf",max,min);

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	double x, y, theta;

	for (int i = 0; i < len; ++i) {
		pa[i].getPoseInMap(x, y, theta);
		PII pi = index(x, y, grain);
		temp = w[i];
		if (temp == 0 || temp == INF) {
			continue;
		} else {
			c.r = (temp - min + 1) / (max - min + 1);
			c.b = (max - temp + 1) / (max - min + 1);
			//c.g = 0.0;
		}
		c.a = 1.0;
		p.x = pi.second * grain;
		p.y = pi.first * grain;
		p.z = 0;
		point.colors.push_back(c);
		point.points.push_back(p);
	}
	pub.publish(point);
}

void CostMarker::PlotPoses(int i, int j, ros::Publisher &pub, double grain) {
	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.ns = "path";

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	c.r = 0.0;
	c.b = 1.0;
	c.g = 1.0;
	c.a = 1.0;
	p.x = (double) j * grain;
	p.y = (double) i * grain;
	//ROS_INFO("Plot point %lf %lf",p.x,p.y);
	p.z = 0;
	point.points.clear();
	point.colors.push_back(c);
	point.points.push_back(p);
	pub.publish(point);

	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.scale.z = 0.1f;
}

void CostMarker::PlotPath(std::vector<PII> path, ros::Publisher &pub, double grain) {

	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.ns = "path";

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	c.r = 0.0;
	c.b = 0.0;
	c.g = 1.0;
	c.a = 1.0;
	//ROS_INFO("Plot point %lf %lf",p.x,p.y);
	p.z = 0;

	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.scale.z = 0.1f;

	for (int i = 0; i < path.size(); ++i) {
		p.x = path[i].second * grain;
		p.y = path[i].first * grain;

		ROS_INFO("Plot point %lf %lf", p.x, p.y);

		point.colors.push_back(c);
		point.points.push_back(p);
	}

	pub.publish(point);

}
